/*

* Copyright (c) 2007, Carnegie Mellon University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of Carnegie Mellon University, nor the
*       names of its contributors may be used to endorse or promote products
*       derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY CARNEGIE MELLON UNIVERSITY ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

  Polar decomposition of a general 3x3 matrix
  Version 2.0
  (this version is identical to version 1.0, but was renamed to 2.0 for consistency reasons)

  This code has been adapted, by Jernej Barbic, from the polar decomposition implementation provided as a companion to the book "Graphics Gems IV": 
  Decompose.c 
  Ken Shoemake, 1993 
  Polar Decomposition of 3x3 matrix in 4x4, M = QS.  
  The Graphics Gems IV implementation is freely available at: 
  http://tog.acm.org/GraphicsGems/
  (This is the official website for Graphics Gems software. The website states that "All code here (on the GraphicsGems website) can be used without restrictions".)

*/

#include <stdio.h>
#include <math.h>
#include "polarDecomposition.h"

// 1-norm of a 3x3 matrix
double PolarDecomposition::norm_one(const double * M)
{
  int i;
  double sum, max;
  max = 0.0;

  for (i=0; i<3; i++) 
  {
    sum = fabs(M[i])+fabs(M[3+i])+fabs(M[6+i]);

    if (max < sum) 
      max = sum;
  }

  return max;
}

// inf-norm of 3x3 matrix
double PolarDecomposition::norm_inf(const double * M)
{
  int i;
  double sum, max;
  max = 0.0;

  for (i=0; i<3; i++) 
  {
    sum = fabs(M[3 * i])+fabs(M[3 * i + 1])+fabs(M[3 * i + 2]);

    if (max < sum) 
      max = sum;
  }

  return max;
}

// M is 3x3 input matrix
// Q is 3x3 rotation output matrix
// S is symmetric 3x3 output matrix
double PolarDecomposition::DoPolarDecomposition(const double * M, double * Q, double * S, double tol)
{
  double Mk[9];
  double MadjTk[9];
  double Ek[9];

  double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;

  int i, j;

  // Mk equals transpose of M
  for(i=0; i<3; i++)
    for(j=0; j<3; j++)
      Mk[3 * i + j] = M[3 * j + i];

  M_one = norm_one(Mk); 
  M_inf = norm_inf(Mk);

  do 
  {
    vcross(&(Mk[3]), &(Mk[6]), &(MadjTk[0])); // cross product of rows 1 and 2
    vcross(&(Mk[6]), &(Mk[0]), &(MadjTk[3]));
    vcross(&(Mk[0]), &(Mk[3]), &(MadjTk[6]));

    det = Mk[0] * MadjTk[0] + Mk[1] * MadjTk[1] + Mk[2] * MadjTk[2];

    if (det==0.0) 
    {
      printf("Warning: zero determinant encountered.\n");
      //do_rank2(Mk, MadjTk, Mk); 

      /*
        // make matrix identity for now
        Q[0] = 1; Q[1] = 0; Q[2] = 0;
        Q[3] = 0; Q[4] = 1; Q[5] = 0;
        Q[6] = 0; Q[7] = 0; Q[8] = 1;
      */

      break;
    }

    MadjT_one = norm_one(MadjTk); 
    MadjT_inf = norm_inf(MadjTk);

    gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));

    g1 = gamma*0.5;
    g2 = 0.5/(gamma*det);

    for(i=0; i<9; i++)
    {
      Ek[i] = Mk[i];
      Mk[i] = g1 * Mk[i] + g2 * MadjTk[i];
      Ek[i] -= Mk[i];
    }

    E_one = norm_one(Ek);
 
    M_one = norm_one(Mk);  
    M_inf = norm_inf(Mk);
  } 
  while ( E_one > M_one * tol );

  // Q = Mk^T 
  for(i=0; i<3; i++)
    for(j=0; j<3; j++)
      Q[3*i+j] = Mk[3*j+i];

  for(i=0; i<3; i++)
    for(j=0; j<3; j++)
    {
      S[3*i+j] = 0;
      for(int k=0; k<3; k++)
        S[3*i+j] += Mk[3*i+k] * M[3*k+j];
    }
    
  // make S symmetric
  for (i=0; i<3; i++) 
    for (j=i; j<3; j++)
      S[3 * i + j] = S[3 * j + i] = 0.5*(S[3 * i + j]+S[3 * j + i]);

  return (det);
}

